%% 组合导航
clear
close all

load("D:\About_code\FlyNueralProject\dataset\P35\20240906\imu_data.mat");
imu = imu_data;
gps=load("D:\About_code\FlyNueralProject\dataset\P35\20240906\gps_data.txt");

L=length(imu);
dTins=0.0055;

%%  卡尔曼滤波参数
Pk1=diag([1,1,1,0.1,0.1,0.1,3e-4,3e-4,3e-4,1e-6,1e-6,1e-6,1e-4,1e-4,1e-4]');
Q0=diag([0,0,0,1e-4,1e-4,1e-4,1e-6,1e-6,1e-6,1e-10,1e-10,1e-10,1e-8,1e-8,1e-8]');
Phi1=eye(15);

R=diag([0.1,0.1,1]');

H=eye(15,15);
H=H(1:3,:);

Q1=zeros(15);

Z1=zeros(3,1);
X1=zeros(15,1);

%% 初始值

macc=mean(imu(1:300,1:3));
yaw0=180;
pitch0=atan2(macc(2),sqrt(macc(1).^2+macc(3).^2))*180/pi;
roll0=atan2(-macc(1),macc(3))*180/pi;
atti1=setoula(yaw0,pitch0,roll0);% 顺序为航向、俯仰、横滚
speed1=[0;0;0];
pos0=[gps(1,2)/180*pi;gps(1,1)/180*pi;0];
[Rmeri,Rprim,ge]=earthmodelupdate(pos0);%根据位置更新地球模型。
pos1=[0;0;0];

dataA=zeros(L,36);
% 
% biasgyro=mean(imu(1:300,4:6))'*(-1)/180*pi;
% biasacc=zeros(3,1);
biasgyro=zeros(3,1);
biasacc=zeros(3,1);

pgps=1;

%% ����
for k=1:1:L
        
    acc=imu(k,1:3)';
    gyro=imu(k,4:6)'/180*pi;
    
    [atti1,speed1,pos1,accn1]=insgyroacc(gyro,acc,atti1,speed1,pos1,dTins,biasacc,biasgyro,ge);    
    Phi1=stateupdate(dTins,atti1,accn1,Phi1);
    Q1=Q1+Q0*dTins;
    
    if(pgps<=length(gps))
        if(imu(k,7)>=gps(pgps,3))    
            gnsspos=[(gps(pgps,1)/180*pi-pos0(2))*Rprim*cos(pos0(1));
            (gps(pgps,2)/180*pi-pos0(1))*Rmeri;
            0];       

            Z1=pos1-gnsspos;       
            [X1,Pk1,Phi1]=kal(Z1,H,Pk1,Q1,R,Phi1);
            Q1=zeros(15);
            pos1=pos1-X1(1:3,1);
            speed1=speed1-X1(4:6,1);
            atti1=qupdate(atti1,(cbn(atti1))'*X1(7:9,1));
            biasgyro=biasgyro-X1(10:12,1);
            biasacc=biasacc-X1(13:15,1);

            pgps=pgps+1;
        end
    end
    
    %数据保存 
    dataA(k,1:9)=[getoula(atti1)',speed1',pos1'];
    dataA(k,10:12)=Z1';
    dataA(k,16:30)=X1';
    dataA(k,31:36)=[biasgyro',biasacc'];
    
   
    
end

%% 绘图
t1=imu(:,7)/1000;
t2=gps(:,3)/1000;
pxb=(gps(:,1)/180*pi-pos0(2))*Rprim*cos(pos0(1));
pyb=(gps(:,2)/180*pi-pos0(1))*Rmeri;



figure
subplot(2,1,1)
plot(t1,dataA(:,7),t2,pxb);
ylabel('位置东');
subplot(2,1,2)
plot(t1,dataA(:,8),t2,pyb);
ylabel('位置北');
legend('组合导航','卫星');
xlabel('时间（s）');


figure
plot(dataA(:,7),dataA(:,8));
hold on
plot(pxb,pyb);
axis equal
title('轨迹');
legend('组合导航','卫星');

figure
subplot(3,1,1)
plot(t1,dataA(:,4));
ylabel('东速/(m/s)');
subplot(3,1,2)
plot(t1,dataA(:,5));
ylabel('北速/(m/s)');
xlabel('时间/s');
subplot(3,1,3)
vel = sqrt(dataA(:,4).^2 + dataA(:,5).^2);
plot(t1,vel);
ylabel('合速度/（m/s）');
xlabel('时间/s');


% 姿态角��
dataA(:,1)=dataA(:,1)+(dataA(:,1)<0)*360;
figure
subplot(3,1,1);
plot(t1,dataA(:,1));
ylabel('yaw/deg');
subplot(3,1,2);
plot(t1,dataA(:,2));
ylabel('pitch/deg');
subplot(3,1,3);
plot(t1,dataA(:,3));
ylabel('roll/deg');
xlabel('时间/s');

figure
subplot(4,1,1);
vel_d = dataA(:,4);
acc_d = zeros(size(vel_d));
acc_d(2:end) = (vel_d(2:end) - vel_d(1:end-1))/dTins;
plot(t1,acc_d);
ylabel('����-acc/m/s^2');
subplot(4,1,2);
vel_b = dataA(:,5);
acc_b = zeros(size(vel_b));
acc_b(2:end) = (vel_b(2:end) - vel_b(1:end-1))/dTins;
plot(t1,acc_b);
ylabel('����-acc/m/s^2');
subplot(4,1,3);
acc = sqrt(acc_d.^2 + acc_b.^2);
plot(t1,acc);
ylabel('|acc|/m/s^2');
subplot(4,1,4);
acc_smooth = smoothdata(acc,'movmedian',200);
plot(t1,acc_smooth);
ylabel('acc-smooth/m/s^2');
xlabel('ʱ��/s');














